#include "SpringAxis.h"
#include "Vec2d.h"
#include <cmath>

SpringAxis::SpringAxis(Vec2d pos, Vec2d dir)
{
	this->pos = pos;
	this->dir = dir.normalizePrecise();
	this->k = 1.0f;
	this->length = 0.0f;
}

SpringAxis::SpringAxis(Vec2d pos, Vec2d dir, float k, float length)
{
	this->pos = pos;
	this->dir = dir.normalizePrecise();
	this->k = k;
	this->length = length;
}

SpringAxis::SpringAxis(Vec2d pos, float radians, float k, float length)
{
	this->pos = pos;
	this->k = k;
	this->dir = Vec2d(cos(radians), -sin(radians));
	this->length = length;
}

void SpringAxis::setDirection(float radians)
{
	this->dir = Vec2d(cos(radians), -sin(radians));
}

float SpringAxis::getDirection()
{
	return atan2(-dir.y, dir.x);
}

void SpringAxis::affect(Entity* e)
{
	Vec2d disp = e->pos - pos;
	float perpDist = fabs(dir.x * disp.y - dir.y * disp.x); //perpendicular distance between the axis and the entity
	float force = (-k) * (perpDist - length);
	Vec2d perpDir(-(dir.y), dir.x);
	if (perpDir * disp < 0)
	{
		perpDir.x = -perpDir.x;
		perpDir.y = -perpDir.y;
	}
	e->accel = e->accel + perpDir * force * e->getInvMass();
}

void SpringAxis::customAffect(Entity* e, float k, float length)
{
	Vec2d disp = e->pos - pos;
	float perpDist = fabs(this->dir.x * disp.y - this->dir.y * disp.x); //perpendicular distance between the axis and the entity
	float force = (-k) * (perpDist - length);
	Vec2d perpDir(-(dir.y), dir.x);
	if (perpDir * disp < 0)
	{
		perpDir.x = -perpDir.x;
		perpDir.y = -perpDir.y;
	}
	e->accel = e->accel + perpDir * force * e->getInvMass();
}